package com.htfyun.serialport.uartservo.AX12_Servo;

import com.htfyun.serialport.uartservo.IServoControl;
import com.htfyun.serialport.uartservo.ServoControlAttr;
import com.htfyun.serialport.uartservo.ServoErrorAttr;
import com.htfyun.serialport.utils.SerialDebugLog;
import com.htfyun.serialport.utils.UsefulUtil;

import java.nio.ByteBuffer;
import java.util.ArrayList;
import java.util.List;
import java.util.UUID;

public class AX12_ServoControl extends IServoControl {

    private final static boolean DEBUG = false;

    public final static byte AX12_SERVO_ID = 1;
    private final static byte AX12_BROADCAST_SERVO_ID = (byte) 0xFE;

    private final static int MAX_POSITION_RAW = 360;
    private final static int MIN_POSITION_RAW = 0;
    private final static int MAX_POSITION_VALUE = 0x3ff;
    private final static int MIN_POSITION_VALUE = 0;

    private final static int MAX_ANGULAR_VELOCITY_RAW = 100;
    private final static int MIN_ANGULAR_VELOCITY_RAW = 0;
    private final static int MAX_ANGULAR_VELOCITY_VALUE = 0x3ff;
    private final static int MIN_ANGULAR_VELOCITY_VALUE = 0;
    private final static int DIRECTION_InfiniteRotate = 1 << 10;


    private final static int MAX_TORQUE_LIMIT_RAW = 100;
    private final static int MIN_TORQUE_LIMIT_RAW = 0;
    private final static int MAX_TORQUE_LIMIT_VALUE = 0x3ff;
    private final static int MIN_TORQUE_LIMIT_VALUE = 0;

    private final static int MAX_LOAD_RAW = 100;
    private final static int MIN_LOAD_RAW = 0;
    private final static int MAX_LOAD_VALUE = 0x3ff;
    private final static int MIN_LOAD_VALUE = 0;
    private final static int LOAD_DIRECTION = 1 << 10;


    public AX12_ServoControl() {
        super(new AX12_ServoCommand(), new AX12_ServoReadDataAnalyze());
    }


    @Override
    public void release() {
        super.release();
    }

    /**
     * position 值范围是 [0, 360], 而我们的AX12 范围是 [0, 0x3ff]. <p>
     * 所以需要转换一下.
     *
     * @param position 值范围是 [0, 360]
     */
    @Override
    public void setPosition(int position) {
        super.setPosition(linearTransferValue(position, MAX_POSITION_RAW, MIN_POSITION_RAW, MAX_POSITION_VALUE, MIN_POSITION_VALUE));
    }

    /**
     * velocity 值范围是 [0, 100], 而我们的AX12 范围是 [0, 0x3ff]. <p>
     * 所以需要转换一下.
     *
     * @param velocity 值范围是 [0, 100]
     */
    @Override
    public void setAngularVelocity(int velocity) {
        super.setAngularVelocity(linearTransferValue(velocity, MAX_ANGULAR_VELOCITY_RAW, MIN_ANGULAR_VELOCITY_RAW, MAX_ANGULAR_VELOCITY_VALUE, MIN_ANGULAR_VELOCITY_VALUE));
    }

    /**
     * angleAreaMin 值范围是 [0, 100], 而我们的AX12 范围是 [0, 0x3ff]. <p>
     * 所以需要转换一下.
     *
     * @param angleAreaMin 值范围是 [0, 100]
     */
    @Override
    public void setAngleAreaMin(int angleAreaMin) {
        super.setAngleAreaMin(linearTransferValue(angleAreaMin, MAX_POSITION_RAW, MIN_POSITION_RAW, MAX_POSITION_VALUE, MIN_POSITION_VALUE));
    }

    /**
     * angleAreaCCW 值范围是 [0, 100], 而我们的AX12 范围是 [0, 0x3ff]. <p>
     * 所以需要转换一下.
     *
     * @param angleAreaMax 值范围是 [0, 100]
     */
    @Override
    public void setAngleAreaMax(int angleAreaMax) {
        int linear = linearTransferValue(angleAreaMax, MAX_POSITION_RAW, MIN_POSITION_RAW, MAX_POSITION_VALUE, MIN_POSITION_VALUE);
        SerialDebugLog.d("angleAreaMax = " + angleAreaMax + ", linear = " + linear);

        super.setAngleAreaMax(linearTransferValue(angleAreaMax, MAX_POSITION_RAW, MIN_POSITION_RAW, MAX_POSITION_VALUE, MIN_POSITION_VALUE));
    }

    /**
     * limit 值范围是 [0, 100], 而我们的AX12 范围是 [0, 0x3ff]. <p>
     * 所以需要转换一下.
     *
     * @param limit 值范围是 [0, 100]
     */
    @Override
    public void setTorqueLimit(int limit) {
        super.setTorqueLimit(linearTransferValue(limit, MAX_TORQUE_LIMIT_RAW, MIN_TORQUE_LIMIT_RAW, MAX_TORQUE_LIMIT_VALUE, MIN_TORQUE_LIMIT_VALUE));
    }

    public void setServoInInfiniteRotateModeEnabled(boolean enabled) {
        servoControl(ServoControlAttr.infinite_rotate_mode, ServoControlAttr.Instruction.set, enabled ? 0 : 0x03ff0000);
    }

    public void getServoInInfiniteRotateModeEnabled() {
        servoControl(ServoControlAttr.infinite_rotate_mode, ServoControlAttr.Instruction.get, 0);
    }

    public void setSpeedWhenInInfiniteRotateMode(int speed, boolean clockwise) {

        if (!servoInterestedInfo.isInInfiniteRotateMode()) {
            SerialDebugLog.w("servo is NOT in infinite rotate mode. but we still set the angular velocity.");
        }

        int speedValue = linearTransferValue(speed, MAX_ANGULAR_VELOCITY_RAW, MIN_ANGULAR_VELOCITY_RAW, MAX_ANGULAR_VELOCITY_VALUE, MIN_ANGULAR_VELOCITY_VALUE);
        if (clockwise) {
            speedValue |= DIRECTION_InfiniteRotate;
        }
        servoControl(ServoControlAttr.angular_velocity, ServoControlAttr.Instruction.set, speedValue);
    }


    private int linearTransferValue(int value, int max_src, int min_src, int max_dst, int min_dst) {

        //y = ax +b

        if (value > max_src) {
            value = max_src;
        }

        if (value < min_src) {
            value = min_src;
        }

        float a = 1.0f * (max_dst - min_dst) / (max_src - min_src);
        float b = (min_dst) - a * min_src;


        float transferValue = a * value + b;

        return (int) (0.5f + transferValue);
    }

    /**
     *
     * @param controlAttr
     * @param instruction
     * @param value <p>如果 instruction 为get时, value没用;</p>
     *              <p>最多传4个byte,(1个int).</p>
     *              //TODO, 如果超过4个byte, 需要再写一个方法来实现
     *
     */
    @Override
    public void servoControl(ServoControlAttr controlAttr, ServoControlAttr.Instruction instruction, int value) {
        AX12_controlTableAttr controlTableAttr = AX12_controlTableAttr.toAX12ControlAttr(controlAttr, instruction);
        AX12_instructionAttr instructionAttr = AX12_instructionAttr.toAX12Instruction(instruction);

        if (controlTableAttr == AX12_controlTableAttr.unknown) {
            return;
        }

        if (instructionAttr == AX12_instructionAttr.none) {
            return;
        }

        servoCommand_setInstruction(instructionAttr);

        switch (instruction) {
            case get:
                servoCommand.setParameters(new byte[] {controlTableAttr.getAddress(), controlTableAttr.getByteCount()});
                break;
            case set:
                if (value < 0) {
                    value = 0;
                }
                byte[] parameters = new byte[1 + controlTableAttr.getByteCount()];
                parameters[0] = controlTableAttr.getAddress();
                for (int i = 0; i < parameters.length - 1; i++) {
                    parameters[i + 1] = (byte)(value >> 8 * i);
                }
                servoCommand.setParameters(parameters);
                break;
        }

        writeServo(servoCommand.buildForCommand());

    }

    @Override
    protected void servoInit() {
        configServoID();
        configVoltageLimit();
        configFeedbackTime();
        configAngularVelocity();

        //初始化时, 获取position值
        getPosition();
        //获取 最小角度限制和最大角度限制
        getAngleAreaMax();
        getAngleAreaMin();
    }

    @Override
    public void servoStop() {
        setTorqueEnabled(false);
    }

    /**
     * @param clockWise true:顺时针(0->360, 向右);false:逆时针(360->0,向左).
     * @param relativePosition 相对角度. 范围是(-360~360)
     */
    @Override
    public void turn(boolean clockWise, int relativePosition) {
        int position = getCurrentPosition();
        if (clockWise) {
            position += relativePosition;

            if (position > servoInterestedInfo.getAngleAreaMax()) {
                position = servoInterestedInfo.getAngleAreaMax();
            }

        } else {
            position -= relativePosition;

            if (position < servoInterestedInfo.getAngleAreaMin()) {
                position = servoInterestedInfo.getAngleAreaMin();
            }
        }

        setPosition(position);
    }

    @Override
    public void onWriteData(UUID uuidToken, ByteBuffer wByteBuffer) {
    }

    @SuppressWarnings("unchecked")
    @Override
    public void onReadData(UUID uuidToken, byte[] readData, int len) {
        if (!uuidToken.equals(this.uuidToken)) {
            return;
        }

        if (len <= 0) {
            return;
        }

        if (readData == null || readData.length < len) {
            return;
        }

        if (DEBUG)
            SerialDebugLog.e("readData = " + UsefulUtil.toHexString(readData, len));

        List<AX12_ReceivedSerialPacket> packetList = readDataAnalyze.analyzeReadData(readData, len);
        if (packetList == null || packetList.isEmpty()) {
            return;
        }

        for (AX12_ReceivedSerialPacket packet : packetList) {
            doActionByAnalyzeReadPacket(packet);
        }

    }

    private void doActionByAnalyzeReadPacket(AX12_ReceivedSerialPacket rPacket) {

        if (rPacket.isValid() && rPacket.getId() != AX12_SERVO_ID) {
            return;
        }

        try {

            SerialDebugLog.e("rPacket = " + rPacket);

            AX12_controlTableAttr controlTableAttr = rPacket.getControlTableAttr();
            if (controlTableAttr == AX12_controlTableAttr.unknown) {
                return;
            }

            ServoControlAttr.Instruction instruction = AX12_instructionAttr.toServoInstruction(rPacket.getInstructionAttr());
            ServoControlAttr controlAttr = AX12_controlTableAttr.toServoControlAttr(controlTableAttr);

            int value = rPacket.getParameterValue();
            SerialDebugLog.e("controlTableAttr = " + controlTableAttr + ", controlAttr = " + controlAttr + ", value = 0x" + Integer.toHexString(value));

            switch (controlAttr) {
                case set_feedback_time:
                    servoInterestedInfo.setFeedbackTime_100ms(value);
                    break;
                case zero_position:
                    servoInterestedInfo.setZeroPosition(value);
                    break;
                case firmware_version:
                    servoInterestedInfo.setFirmwareVersion(value);
                    break;
                case position:
                    if (isFeedbackEnabled() || instruction == ServoControlAttr.Instruction.get) {
                        servoInterestedInfo.setPosition(linearTransferValue(value & MAX_POSITION_VALUE,
                                MAX_POSITION_VALUE, MIN_POSITION_VALUE, MAX_POSITION_RAW, MIN_POSITION_RAW));
                    }

                    if (instruction == ServoControlAttr.Instruction.get) {
                        servoInterestedInfo.setMoving((value & 0x8000) != 0);
                    }
                    break;
                case angular_velocity:
                    servoInterestedInfo.setAngularVelocity(linearTransferValue(value & MAX_ANGULAR_VELOCITY_VALUE,
                            MAX_ANGULAR_VELOCITY_VALUE, MIN_ANGULAR_VELOCITY_VALUE, MAX_ANGULAR_VELOCITY_RAW, MIN_ANGULAR_VELOCITY_RAW));

                    servoInterestedInfo.setInfiniteRotateClockWise(0 != (value & DIRECTION_InfiniteRotate));

                    break;
                case angle_area_min:
                    servoInterestedInfo.setAngleAreaMin(linearTransferValue(value & MAX_POSITION_VALUE,
                            MAX_POSITION_VALUE, MIN_POSITION_VALUE, MAX_POSITION_RAW, MIN_POSITION_RAW));

                    break;
                case angle_area_max:
                    servoInterestedInfo.setAngleAreaMax(linearTransferValue(value & MAX_POSITION_VALUE,
                            MAX_POSITION_VALUE, MIN_POSITION_VALUE, MAX_POSITION_RAW, MIN_POSITION_RAW));

                    break;
                case infinite_rotate_mode:
                    servoInterestedInfo.setInInfiniteRotateMode(value == 0);
                    break;
                case torque_limit:

                    servoInterestedInfo.setTorqueLimit(linearTransferValue(value & MAX_TORQUE_LIMIT_VALUE,
                            MAX_TORQUE_LIMIT_VALUE, MIN_TORQUE_LIMIT_VALUE, MAX_TORQUE_LIMIT_RAW, MIN_TORQUE_LIMIT_RAW));

                    break;
                case torque_active:
                    servoInterestedInfo.setTorqueEnabled(value != 0);
                    break;
                case load:
                    servoInterestedInfo.setLoad(linearTransferValue(value & MAX_LOAD_VALUE,
                            MAX_LOAD_VALUE, MIN_LOAD_VALUE, MAX_LOAD_RAW, MIN_LOAD_RAW));
                    servoInterestedInfo.setLoadClockWise(0 != (value & LOAD_DIRECTION));
                    break;
                case temperature:
                    servoInterestedInfo.setTemperature(value);
                    break;
                case move_status:
                    servoInterestedInfo.setMoving(value != 0);
                    break;
                case unknown:
                default:break;

            }

            List<ServoErrorAttr> servoErrorAttrList = null;
            List<AX12_errorAttr> ax12_errorAttrList = rPacket.getErrorAttr();
            if (ax12_errorAttrList != null && !ax12_errorAttrList.isEmpty()) {
                servoErrorAttrList = new ArrayList<>();
                for (AX12_errorAttr errorAttr : ax12_errorAttrList) {
                    servoErrorAttrList.add(AX12_errorAttr.toServoErrorAttr(errorAttr));
                }
            }

            onServoListenerAction(servoErrorAttrList, controlAttr, instruction);

        } catch (Exception e) {
            e.printStackTrace();

        }

    }

    private void configServoID() {

        byte id = AX12_SERVO_ID;
        servoCommand.setId(new byte[]{AX12_BROADCAST_SERVO_ID});
        servoCommand.setInstruction(new byte[]{AX12_instructionAttr.write.getInstruction()});
        servoCommand.setParameters(new byte[]{AX12_controlTableAttr.servo_id.getAddress(), id});

        writeServo(servoCommand.buildForCommand());

        servoCommand.setId(new byte[]{id});
    }

    public void configVoltageLimit() {

        byte upper_limit = 100;//10V
        byte lower_limit = 30; //3V
        servoCommand.setInstruction(new byte[]{AX12_instructionAttr.write.getInstruction()});
        servoCommand.setParameters(new byte[]{AX12_controlTableAttr.voltage_upper_limit.getAddress(), upper_limit});
        writeServo(servoCommand.buildForCommand());

        servoCommand.setParameters(new byte[]{AX12_controlTableAttr.voltage_lower_limit.getAddress(), lower_limit});
        writeServo(servoCommand.buildForCommand());
    }

    private void configFeedbackTime() {
        setFeedbackTime_100ms(DEFAULT_FEEDBACK_TIME_100ms);
    }

    private void configAngularVelocity() {
        setAngularVelocity(DEFAULT_ANGULAR_VELOCITY);
    }

    private void servoCommand_setInstruction(AX12_instructionAttr attr) {
        servoCommand.setInstruction(new byte[]{attr.getInstruction()});
    }

}
